//==================================================================
//  Copyright (C) 2006  Davide Pasca
//
//  This library is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.
//
//  This library is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//  Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with this library; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
//==================================================================
/// @file	pvector.h
///
/// @brief	Math vector/point/array classes.
///
//==================================================================

#ifndef PVECTOR_H
#define PVECTOR_H

#define _USE_MATH_DEFINES
#include <math.h>
#include <math.h>
#include <float.h>

//==================================================================
namespace PMath
{

//==================================================================
struct Vector2
{
	union {
		struct
		{
			float	_x;
			float	_y;
		};
		float	_v[2];
	};

	Vector2() { }

	Vector2( float x, float y ) : _x(x), _y(y)
	{
	}

	Vector2( const float xy[3] ) : _x(xy[0]), _y(xy[1])
	{
	}

	void Set( float x, float y )
	{
		_x = x;
		_y = y;
	}

	void Set( float xy[2] )
	{
		_x = xy[0];
		_y = xy[1];
	}

	void SetZero()
	{
		_x = _y = 0;
	}

	void SetMinValue()
	{
		_x = _y = -FLT_MAX;
	}

	void SetMaxValue()
	{
		_x = _y = FLT_MAX;
	}

	void SetMin( const Vector2 &a )
	{
		_x = _x < a._x ? _x : a._x;
		_y = _y < a._y ? _y : a._y;
	}
	void SetMax( const Vector2 &a )
	{
		_x = _x > a._x ? _x : a._x;
		_y = _y > a._y ? _y : a._y;
	}

	float GetDot( const Vector2 &rightvec ) const
	{
		return	_x * rightvec._x +
				_y * rightvec._y;
	}

	Vector2 GetSquared() const
	{
		return Vector2( _x * _x, _y * _y );
	}

	float GetLengthSqr() const
	{
		return _x*_x + _y*_y;
	}

	float GetLength() const
	{
		return sqrtf( _x*_x + _y*_y );
	}

	Vector2 GetNormalized() const
	{
		float	oolen = 0;
		float	len = GetLength();
		if ( len )
			oolen = 1.0f / len;

		return Vector2( _x * oolen, _y * oolen );
	}

	void Normalize()
	{
		*this = this->GetNormalized();
	}

	Vector2 GetLerp( const Vector2 &dest, float t ) const
	{
		return *this * (t-1) + dest * t;
	}

	Vector2	operator - () const						{ return Vector2( -_x, -_y );	}
	Vector2 operator + (const Vector2 &vr) const 	{ return Vector2( _x + vr._x, _y + vr._y );	}
	Vector2 operator - (const Vector2 &vr) const 	{ return Vector2( _x - vr._x, _y - vr._y );	}
	Vector2 operator * (const Vector2 &vr) const 	{ return Vector2( _x * vr._x, _y * vr._y );	}
	Vector2 operator + ( float scalar )		const 	{ return Vector2( _x + scalar, _y + scalar );	}
	Vector2 operator - ( float scalar )		const 	{ return Vector2( _x - scalar, _y - scalar );	}
	Vector2 operator * ( float scalar )		const 	{ return Vector2( _x * scalar, _y * scalar );	}
	void	operator +=( const Vector2 &vr ){ _x += vr._x; _y += vr._y; }
	void	operator -=( const Vector2 &vr ){ _x -= vr._x; _y -= vr._y; }
	void	operator *=( const Vector2 &vr ){ _x *= vr._x; _y *= vr._y; }
	void	operator +=( float a )			{ _x += a; _y += a; }
	void	operator -=( float a )			{ _x -= a; _y -= a; }
	void	operator *=( float a )			{ _x *= a; _y *= a; }
};


//==================================================================
struct Vector3
{
	union {
		struct
		{
			float	_x;
			float	_y;
			float	_z;
		};
		float	_v[3];
	};

	Vector3() { }

	Vector3( float x, float y, float z ) : _x(x), _y(y), _z(z)
	{
	}

	Vector3( const float xyz[3] ) : _x(xyz[0]), _y(xyz[1]), _z(xyz[2])
	{
	}

	void Set( float x, float y, float z )
	{
		_x = x;
		_y = y;
		_z = z;
	}

	void Set( float xyz[3] )
	{
		_x = xyz[0];
		_y = xyz[1];
		_z = xyz[2];
	}

	void SetZero()
	{
		_x = _y = _z = 0;
	}

	void SetMinValue()
	{
		_x = _y = _z = -FLT_MAX;
	}

	void SetMaxValue()
	{
		_x = _y = _z = FLT_MAX;
	}

	void SetMin( const Vector3 &a )
	{
		_x = _x < a._x ? _x : a._x;
		_y = _y < a._y ? _y : a._y;
		_z = _z < a._z ? _z : a._z;
	}
	void SetMax( const Vector3 &a )
	{
		_x = _x > a._x ? _x : a._x;
		_y = _y > a._y ? _y : a._y;
		_z = _z > a._z ? _z : a._z;
	}

	float GetDot( const Vector3 &rightvec ) const
	{
		return _x * rightvec._x +
			   _y * rightvec._y +
			   _z * rightvec._z;
	}

	Vector3 GetCross( const Vector3 &rightvec ) const
	{
		return Vector3( _y * rightvec._z - rightvec._y * _z,
						_z * rightvec._x - rightvec._z * _x,
						_x * rightvec._y - rightvec._x * _y );
	}

	Vector3 GetSquared() const
	{
		return Vector3( _x * _x, _y * _y, _z * _z );
	}

	float GetLengthSqr() const
	{
		return _x*_x + _y*_y + _z*_z;
	}

	float GetLength() const
	{
		return sqrtf( _x*_x + _y*_y + _z*_z );
	}

	Vector3 GetNormalized() const
	{
		float	oolen = 0;
		float	len = GetLength();
		if ( len )
			oolen = 1.0f / len;

		return Vector3( _x * oolen, _y * oolen, _z * oolen );
	}

	void Normalize()
	{
		*this = this->GetNormalized();
	}

	Vector3 GetLerp( const Vector3 &dest, float t ) const
	{
		return *this * (t-1) + dest * t;
	}


	Vector3	operator - () const						{ return Vector3( -_x, -_y, -_z );	}
	Vector3 operator + (const Vector3 &vr) const 	{ return Vector3( _x + vr._x, _y + vr._y, _z + vr._z );	}
	Vector3 operator - (const Vector3 &vr) const 	{ return Vector3( _x - vr._x, _y - vr._y, _z - vr._z );	}
	Vector3 operator * (const Vector3 &vr) const 	{ return Vector3( _x * vr._x, _y * vr._y, _z * vr._z );	}
	Vector3 operator + ( float scalar )		const 	{ return Vector3( _x + scalar, _y + scalar, _z + scalar );	}
	Vector3 operator - ( float scalar )		const 	{ return Vector3( _x - scalar, _y - scalar, _z - scalar );	}
	Vector3 operator * ( float scalar )		const 	{ return Vector3( _x * scalar, _y * scalar, _z * scalar );	}
	void	operator +=( const Vector3 &vr ){ _x += vr._x; _y += vr._y; _z += vr._z; }
	void	operator -=( const Vector3 &vr ){ _x -= vr._x; _y -= vr._y; _z -= vr._z; }
	void	operator *=( const Vector3 &vr ){ _x *= vr._x; _y *= vr._y; _z *= vr._z; }
	void	operator +=( float a )			{ _x += a; _y += a; _z += a; }
	void	operator -=( float a )			{ _x -= a; _y -= a; _z -= a; }
	void	operator *=( float a )			{ _x *= a; _y *= a; _z *= a; }
};


//==================================================================
struct Vector4_nal
{
	union {
		struct
		{
			float	_x;
			float	_y;
			float	_z;
			float	_w;
		};
		float	_v[4];
	};

	Vector4_nal() { }

	Vector4_nal( float x, float y, float z, float w ) : _x(x), _y(y), _z(z), _w(w)
	{
	}

	Vector4_nal( const Vector3 &a, float w ) : _x(a._x), _y(a._y), _z(a._z), _w(w)
	{
	}

	Vector4_nal( const float xyzw[4] ) : _x(xyzw[0]), _y(xyzw[1]), _z(xyzw[2]), _w(xyzw[3])
	{
	}

	const Vector3 &GetVector3() const
	{
		return *(Vector3 *)this;
	}

	void Set( float x, float y, float z, float w=1 )
	{
		_x = x;
		_y = y;
		_z = z;
		_w = w;
	}

	void Set( float xyz[3], float w=1 )
	{
		_x = xyz[0];
		_y = xyz[1];
		_z = xyz[2];
		_w = w;
	}

	void SetZero()
	{
		_x = _y = _z = _w = 0;
	}

	void SetMinValue()
	{
		_x = _y = _z = _w = -FLT_MAX;
	}

	void SetMaxValue()
	{
		_x = _y = _z = _w = FLT_MAX;
	}

	void SetMin( const Vector4_nal &a )
	{
		_x = _x < a._x ? _x : a._x;
		_y = _y < a._y ? _y : a._y;
		_z = _z < a._z ? _z : a._z;
		_w = _w < a._w ? _w : a._w;
	}
	void SetMax( const Vector4_nal &a )
	{
		_x = _x > a._x ? _x : a._x;
		_y = _y > a._y ? _y : a._y;
		_z = _z > a._z ? _z : a._z;
		_w = _w > a._w ? _w : a._w;
	}

	float GetDot( const Vector4_nal &rightvec ) const
	{
		return _x * rightvec._x + 
			   _y * rightvec._y +
			   _z * rightvec._z +
			   _w * rightvec._w;
	}

	float GetDot( const Vector3 &rightvec ) const
	{
		return _x * rightvec._x +
			   _y * rightvec._y +
			   _z * rightvec._z;
	}

	Vector4_nal GetCross3( const Vector4_nal &rightvec ) const
	{
		return Vector4_nal( _y * rightvec._z - rightvec._y * _z,
						_z * rightvec._x - rightvec._z * _x,
						_x * rightvec._y - rightvec._x * _y,
						_w );
	}

	Vector4_nal GetSquared() const
	{
		return Vector4_nal( _x * _x, _y * _y, _z * _z, _w * _w );
	}

	float GetLength3() const
	{
		return sqrtf( _x*_x + _y*_y + _z*_z );
	}
	float GetLength4() const
	{
		return sqrtf( _x*_x + _y*_y + _z*_z + _w*_w );
	}

	float GetLengthSqr3() const
	{
		return _x*_x + _y*_y + _z*_z;
	}
	float GetLengthSqr4() const
	{
		return _x*_x + _y*_y + _z*_z + _w*_w;
	}

	Vector4_nal GetNormalized3() const
	{
		float	oolen = 0;
		float	len = GetLength3();
		if ( len )
			oolen = 1.0f / len;

		return Vector4_nal( _x * oolen, _y * oolen, _z * oolen, _w );
	}

	Vector4_nal GetNormalized4() const
	{
		float	oolen = 0;
		float	len = GetLength4();
		if ( len )
			oolen = 1.0f / len;

		return Vector4_nal( _x * oolen, _y * oolen, _z * oolen, _w * oolen );
	}

	Vector4_nal GetLerp( const Vector4_nal &dest, float t ) const
	{
		return *this * (t-1) + dest * t;
	}

	void Normalize3()	{	*this = this->GetNormalized3();	}
	void Normalize4()	{	*this = this->GetNormalized4();	}

	Vector4_nal	operator - () const						{ return Vector4_nal( -_x, -_y, -_z, -_w );	}
	Vector4_nal operator + (const Vector4_nal &vr)	const 	{ return Vector4_nal( _x + vr._x, _y + vr._y, _z + vr._z, _w + vr._w );	}
	Vector4_nal operator - (const Vector4_nal &vr)	const 	{ return Vector4_nal( _x - vr._x, _y - vr._y, _z - vr._z, _w - vr._w );	}
	Vector4_nal operator * (const Vector4_nal &vr)	const 	{ return Vector4_nal( _x * vr._x, _y * vr._y, _z * vr._z, _w * vr._w );	}
	Vector4_nal operator + ( float scalar )		const 	{ return Vector4_nal( _x + scalar, _y + scalar, _z + scalar, _w + scalar );	}
	Vector4_nal operator - ( float scalar )		const 	{ return Vector4_nal( _x - scalar, _y - scalar, _z - scalar, _w - scalar );	}
	Vector4_nal operator * ( float scalar )		const 	{ return Vector4_nal( _x * scalar, _y * scalar, _z * scalar, _w * scalar );	}
	void	operator +=( const Vector4_nal &vr ){ _x += vr._x; _y += vr._y; _z += vr._z; _w += vr._w; }
	void	operator -=( const Vector4_nal &vr ){ _x -= vr._x; _y -= vr._y; _z -= vr._z; _w -= vr._w; }
	void	operator *=( const Vector4_nal &vr ){ _x *= vr._x; _y *= vr._y; _z *= vr._z; _w *= vr._w; }
	void	operator +=( float a )			{ _x += a; _y += a; _z += a; _w += a; }
	void	operator -=( float a )			{ _x -= a; _y -= a; _z -= a; _w -= a; }
	void	operator *=( float a )			{ _x *= a; _y *= a; _z *= a; _w *= a; }
};

//==================================================================
typedef Vector4_nal	__declspec(align(16)) Vector4;

//==================================================================
struct Quatern_nal
{
	enum InitMode
	{
		IDENTITY
	};

	Vector3	_v3;
	float	_w;

	Quatern_nal(){}
	Quatern_nal( InitMode initmode )
	{
		PASSERT( initmode == IDENTITY );
		Identity();
	}
	Quatern_nal( const Vector3 &v, float w ) :
		_v3(v),
		_w(w)
	{
	}

	PFORCEINLINE void Identity()
	{
		_v3.SetZero();
		_w = 1;
	}

	PFORCEINLINE Quatern_nal GetConjugate() const
	{
		return Quatern_nal( -_v3, _w );
	}

	PFORCEINLINE void SetNorAxisAngle( const Vector3 &axis, float angle )
	{
		float hangle = angle * 0.5f;
		_v3 = axis * sinf( hangle );
		_w	= cosf( hangle );
	}

	PFORCEINLINE void SetNorAxisAngle( const Vector4 &axis_wang )
	{
		SetNorAxisAngle( axis_wang.GetVector3(), axis_wang._w );
	}

	const Vector4	&GetV4() const
	{
		return *(const Vector4 *)this;
	}

	Vector4			&GetV4()
	{
		return *(Vector4 *)this;
	}

	Quatern_nal operator + (const Quatern_nal &B) const
	{
		const Quatern_nal	&A = *this;
		return Quatern_nal( A._v3 + B._v3, A._w + B._w );
	}

	PFORCEINLINE Quatern_nal operator * (const Quatern_nal &B) const
	{
		const Quatern_nal	&A = *this;
		return Quatern_nal(	A._v3.GetCross( B._v3 ) + B._v3 * A._w + A._v3 * B._w,
						A._w * B._w - A._v3.GetDot( B._v3 ) );
	}

};

//==================================================================
typedef Quatern_nal	__declspec(align(16)) Quatern;

//==================================================================
};

#endif
